package telemetry

import (
	"fmt"
	"io"
	"math"
	"time"

	"github.com/paulmach/go.geo"
)

// Represents one second of telemetry data
type TELEM struct {
	Accl        []ACCL
	Gps         []GPS5
	Gyro        []GYRO
	GpsFix      GPSF
	GpsAccuracy GPSP
	Time        GPSU
	Temp        TMPC
}

// the thing we want, json-wise
// GPS data might have a generated timestamp and derived track
type TELEM_OUT struct {
	*GPS5

	GpsAccuracy uint16  `json:"gps_accuracy,omitempty"`
	GpsFix      uint32  `json:"gps_fix,omitempty"`
	Temp        float32 `json:"temp,omitempty"`
	Track       float64 `json:"track,omitempty"`
}

var pp *geo.Point = geo.NewPoint(10, 10)
var last_good_track float64 = 0

// zeroes out the telem struct
func (t *TELEM) Clear() {
	t.Accl = t.Accl[:0]
	t.Gps = t.Gps[:0]
	t.Gyro = t.Gyro[:0]
	t.Time.Time = time.Time{}
}

// determines if the telem has data
func (t *TELEM) IsZero() bool {
	// hack.
	return t.Time.Time.IsZero()
}

// try to populate a timestamp for every GPS row. probably bogus.
func (t *TELEM) FillTimes(until time.Time) error {
	len := len(t.Gps)
	diff := until.Sub(t.Time.Time)

	offset := diff.Seconds() / float64(len)

	for i, _ := range t.Gps {
		dur := time.Duration(float64(i)*offset*1000) * time.Millisecond
		ts := t.Time.Time.Add(dur)
		t.Gps[i].TS = ts.UnixNano() / 1000
	}

	return nil
}

// try to populate a timestamp for every Gyro/Accl row. probably bogus.
func (t *TELEM) FillTimesGyroAccl(until time.Time) error {
	len := len(t.Gyro)
	diff := until.Sub(t.Time.Time)

	offset := diff.Seconds() / float64(len)

	for i, _ := range t.Gyro {
		dur := time.Duration(float64(i)*offset*1000) * time.Millisecond
		ts := t.Time.Time.Add(dur)
		t.Gyro[i].TS = ts.UnixNano() / 1000
	}

	return nil
}

//计算公式
//C = sin(LatA*Pi/180)*sin(LatB*Pi/180) + cos(LatA*Pi/180)*cos(LatB*Pi/180)*cos((MLonA-MLonB)*Pi/180)
//Distance = R*Arccos(C)*Pi/180
func EarthDistance(lat1, lng1, lat2, lng2 float64) float64 {
	if lat1 == 0 {
		return 0
	}
	radius := 6378137.0
	rad := math.Pi / 180.0
	lat1 = lat1 * rad
	lng1 = lng1 * rad
	lat2 = lat2 * rad
	lng2 = lng2 * rad
	theta := lng2 - lng1
	dist := math.Acos(math.Sin(lat1)*math.Sin(lat2) + math.Cos(lat1)*math.Cos(lat2)*math.Cos(theta))
	ret := dist * radius
	if math.IsNaN(ret) {
		ret = 0
	}
	return ret
}

func (t *TELEM) ShitJson() []TELEM_OUT {
	var out []TELEM_OUT

	for i, _ := range t.Gps {
		jobj := TELEM_OUT{&t.Gps[i], 0, 0, 0, 0}
		if 0 == i {
			jobj.GpsAccuracy = t.GpsAccuracy.Accuracy
			jobj.GpsFix = t.GpsFix.F
			jobj.Temp = t.Temp.Temp
		}

		p := geo.NewPoint(jobj.GPS5.Longitude, jobj.GPS5.Latitude)
		jobj.Track = pp.BearingTo(p)
		pp = p

		if jobj.Track < 0 {
			jobj.Track = 360 + jobj.Track
		}

		// only set the track if speed is over 1 m/s
		// if it's slower (eg, stopped) it will drift all over with the location
		if jobj.GPS5.Speed > 1 {
			last_good_track = jobj.Track
		} else {
			jobj.Track = last_good_track
		}

		out = append(out, jobj)
	}

	return out
}

func (t *TELEM) ShitCsv(w io.Writer, splt string, prev *TELEM, accu *float64, startm int64) bool {
	var lat0, lng0 float64
	if !prev.IsZero() {
		n := len(prev.Gps)
		lat0, lng0 = prev.Gps[n-1].Latitude, prev.Gps[n-1].Longitude
	}

	for i, _ := range t.Gps {
		jobj := TELEM_OUT{&t.Gps[i], 0, 0, 0, 0}
		if 0 == i {
			jobj.GpsAccuracy = t.GpsAccuracy.Accuracy
			jobj.GpsFix = t.GpsFix.F
			jobj.Temp = t.Temp.Temp
		}

		p := geo.NewPoint(jobj.GPS5.Longitude, jobj.GPS5.Latitude)
		jobj.Track = pp.BearingTo(p)
		pp = p

		if jobj.Track < 0 {
			jobj.Track = 360 + jobj.Track
		}

		// only set the track if speed is over 1 m/s
		// if it's slower (eg, stopped) it will drift all over with the location
		if jobj.GPS5.Speed > 1 {
			last_good_track = jobj.Track
		} else {
			jobj.Track = last_good_track
		}

		dist := EarthDistance(lat0, lng0, jobj.Latitude, jobj.Longitude)
		lat0, lng0 = jobj.Latitude, jobj.Longitude
		*accu += dist

		tm := time.Unix(int64(int(jobj.TS/1000000)), jobj.TS%1000000*1000)
		fstr := "%v" + splt + "%.3f" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%.3f" + splt + "%.3f" + "\n"
		fmt.Fprintf(w, fstr, tm.Format("2006-01-02T15:04:05.000"), float64(jobj.TS-startm)/1000000, jobj.Longitude, jobj.Latitude, jobj.Altitude,
			jobj.Speed, jobj.Track, dist, *accu)
	}

	return true
}

func (t *TELEM) ShitCsvGyroAccl(w io.Writer, splt string, startm int64) bool {
	for i, _ := range t.Gyro {
		tm := time.Unix(int64(int(t.Gyro[i].TS/1000000)), t.Gyro[i].TS%1000000*1000)
		fstr := "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + splt + "%v" + "\n"
		fmt.Fprintf(w, fstr, tm.Format("2006-01-02T15:04:05.000"), float64(t.Gyro[i].TS-startm)/1000000,
			t.Gyro[i].X, t.Gyro[i].Y, t.Gyro[i].Z,
			t.Accl[i].X, t.Accl[i].Y, t.Accl[i].Z)
	}

	return true
}
